//vel2pwm_rated节点，订阅/cmd_vel，定时发布/pwm。
//有用的数据仅为x轴速度和z轴角速度
// 打角115为正，160左40度，70右40度
// 速度1520不动，1525为0.2但有响声，1530为0.6，1540为0.7，1550为1.0，1560可到1.8左右不过加速还要一段距离，1565为2左右

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>
#include<iostream>
#include<fstream>
#include<string>
using namespace std;
double speed;
double angle;

void TwistCallback(const geometry_msgs::Twist& twist)
{
    angle=twist.angular.z;
    speed=twist.linear.x;
}

void DistanceCallback(const std_msgs::Float64& distance)
{
    diatance.data
}

double speedcontrol(double angle)
{
    double speedpwm;
 //   if(angle>=0)speedpwm=1570-25*angle;
 //   else    speedpwm=1570+25*angle;
    if(angle >= 0.7)
    {
        speedpwm = 1555;
    }
    else if(angle <= -0.7)
    {
        speedpwm = 1555;
    }
    else if(angle >= 0.35)
    {
        speedpwm = 1555;
    }
    else if(angle <= -0.35)
    {
        speedpwm = 1555;
    }
        else if(angle >= 0.15)
    {
        speedpwm = 1560;
    }
    else if(angle <= -0.15)
    {
        speedpwm = 1560;
    }
    else 
    {
        speedpwm = 1565;
    }
    
    return speedpwm;

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vel2pwm");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/cmd_vel",1,TwistCallback);
    ros::Subscriber sub2 = nh.subscribe("/distance",1,DistanceCallback);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>( "/pwm", 1 );
    geometry_msgs::Twist pwm;
    ROS_INFO("vel2pwm_rated start successfully.");
    // double t0=ros::Time::now().toSec();
    ros::Rate r(16);
    int distance;
    int temp[1024] = {0};
    int create_flag = 0;
    while(ros::ok())
    {
        ros::spinOnce();
        if(speed!=0)
        {
            pwm.linear.x=speedcontrol(angle);
        }
        else
        {
            pwm.linear.x=1490;
        }
        if(angle >= 0.7) pwm.angular.z = 160; //限幅左右40度
        else if(angle <= -0.7) pwm.angular.z = 70;
        else pwm.angular.z=angle*64.46+115; //*180/pi*45/40，即弧度制转角度制再转pwm

        distance = (int)diatance.data * 100; //将里程化为厘米单位制
        if(distance % 5 == 0 && 0 < distance < 1000)
        {
            temp[distance / 5] = angle; //每五厘米记录一下打角
        }
        if(distance > 1000 && create_flag == 0)
        {
            create_flag = 1;
            ofstream OutFile("Road.txt"); //创建一个road.txt
            for(int i = 0; i < temp.size() - 1;i++)
            {
                OutFile << to_string(temp[i]);
                // OutFile << to_string(angle);
                OutFile << "\n";
            }
            OutFile.close();
        }

        pub.publish(pwm);
        r.sleep();
    }
    // ifstream readFile("Road.txt");

    // string line;
    // int p;
    // if(readFile) // 有该文件
	// {
	// 	while (getline (readFile, line)) // line中不包括每行的换行符
	// 	{ 
    //         p = stoi(line);
	// 		// cout << line << endl;
    //         printf("angle = %d\n", p);
	// 	}
	// }
	// else // 没有该文件
	// {
	// 	cout <<"no such file" << endl;
	// }

    return 0;
}